﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Diagnostics;
using TomanuExtensions;

namespace RaytracerLib.MathLib.Polynomials
{
    public struct PolynomialSolverAlgebraic
    {
        public readonly Polynomial Polynomial;

        public PolynomialSolverAlgebraic(Polynomial a_polynomial)
        {
            Polynomial = a_polynomial.RemoveZeroes();
        }

        public double SolveAlgebraicLinear()
        {
            Debug.Assert(Polynomial.Order == 1);

            return -Polynomial[0] / Polynomial[1];
        }

        public double SolveAlgebraicQuarticOne()
        {
            double x1 = 0, x2 = 0, x3 = 9, x4 = 0;
            int roots = SolveAlgebraicQuartic(ref x1, ref x2, ref x3, ref x4);

            if (roots == 0)
                return Double.PositiveInfinity;

            double result = double.PositiveInfinity;

            if ((x1 > Constants.MINIMAL_DISTANT) && (x1 < result))
                result = x1;

            if (roots >= 2)
            {
                if ((x2 > Constants.MINIMAL_DISTANT) && (x2 < result))
                    result = x2;
            }

            if (roots >= 3)
            {
                if ((x3 > Constants.MINIMAL_DISTANT) && (x3 < result))
                    result = x3;
            }

            if (roots == 4)
            {
                if ((x4 > Constants.MINIMAL_DISTANT) && (x4 < result))
                    result = x4;
            }

            return result;
        }

        public double SolveAlgebraicQuadraticOne()
        {
            double x1 = 0, x2 = 0;
            int roots = SolveAlgebraicQuadratic(ref x1, ref x2);

            if (roots == 0)
                return Double.PositiveInfinity;

            double result = double.PositiveInfinity;

            if ((x1 > Constants.MINIMAL_DISTANT) && (x2 < result))
                result = x1;

            if (roots >= 2)
            {
                if ((x2 > Constants.MINIMAL_DISTANT) && (x2 < result))
                    result = x2;
            }

            return result;
        }

        public int SolveAlgebraicQuartic(ref double a_x1, ref double a_x2, ref double a_x3, ref double a_x4)
        {
            if (Polynomial.Order < 4)
            {
                int roots3 = SolveAlgebraicCubic(ref a_x1, ref a_x2, ref a_x3);
                a_x4 = Double.NaN;
                return roots3;
            }

            if ((Polynomial[4] != 1) && (Polynomial[4] != 0))
            {
                return new PolynomialSolverAlgebraic(Polynomial.Normalize()).SolveAlgebraicQuartic(
                    ref a_x1, ref a_x2, ref a_x3, ref a_x4);
            }

            double B2 = Polynomial[3] * Polynomial[3];
            double p = -3 * B2 / 8 + Polynomial[2];
            double q = B2 * Polynomial[3] / 8 - Polynomial[3] * Polynomial[2] / 2 + Polynomial[1];
            double r = -3 * B2 * B2 / 256 + B2 * Polynomial[2] / 16 - Polynomial[3] * Polynomial[1] / 4 + Polynomial[0];

            double resubstitute = -Polynomial[3] / 4;
            int roots = 0;

            if (r.IsAlmostEquals(0, Constants.ALGEBRAIC_QUARTIC_SOLVER_QUARTIC_PRECISION))
            {
                PolynomialSolverAlgebraic cubic_solver = 
                    new PolynomialSolverAlgebraic(new Polynomial(q, p, 0, 1));
                double x1 = 0, x2 = 0, x3 = 0;
                int cubic_roots = cubic_solver.SolveAlgebraicCubic(ref x1, ref x2, ref x3);
                if (cubic_roots >= 1)
                {
                    double x = x1 + resubstitute;
                    switch (roots)
                    {
                        case 0: a_x1 = x; break;
                        case 1: a_x2 = x; break;
                        case 2: a_x3 = x; break;
                        case 3: a_x4 = x; break;
                    }
                    roots++;
                }
                if (cubic_roots >= 2)
                {
                    double x = x2 + resubstitute;
                    switch (roots)
                    {
                        case 0: a_x1 = x; break;
                        case 1: a_x2 = x; break;
                        case 2: a_x3 = x; break;
                        case 3: a_x4 = x; break;
                    }
                    roots++;
                }
                if (cubic_roots == 3)
                {
                    double x = x3 + resubstitute;
                    switch (roots)
                    {
                        case 0: a_x1 = x; break;
                        case 1: a_x2 = x; break;
                        case 2: a_x3 = x; break;
                        case 3: a_x4 = x; break;
                    }
                    roots++;
                }

                switch (roots)
                {
                    case 0: a_x1 = resubstitute; break;
                    case 1: a_x2 = resubstitute; break;
                    case 2: a_x3 = resubstitute; break;
                    case 3: a_x4 = resubstitute; break;
                }
                roots++;
            }
            else
            {
                double z = 0;

                PolynomialSolverAlgebraic cubic_solver = new PolynomialSolverAlgebraic(
                    new Polynomial(r * p / 2 - q * q / 8, -r, -p /2, 1));
                double x1 = 0, x2 = 0, x3 = 0;
                int cubic_roots = cubic_solver.SolveAlgebraicCubic(ref x1, ref x2, ref x3);

                double u = -1;
                double v = -1;

                for (int i = 0; i < cubic_roots; i++)
                {
                    switch (i)
                    {
                        case 0: z = x1; break;
                        case 1: z = x2; break;
                        default: z = x3; break;
                    }

                    u = z * z - r;

                    if (u.IsAlmostEquals(0, Constants.ALGEBRAIC_QUARTIC_SOLVER_QUARTIC_PRECISION))
                        u = 0;
                    else if (u > 0)
                        u = Math.Sqrt(u);
                    else
                        continue;

                    v = 2 * z - p;

                    if (v.IsAlmostEquals(0, Constants.ALGEBRAIC_QUARTIC_SOLVER_QUARTIC_PRECISION))
                        v = 0;
                    else if (v > 0)
                        v = Math.Sqrt(v);
                    else
                        continue;

                    break;
                }

                if (u < 0)
                    return 0;
                if (v < 0)
                    return 0;

                double b = q < 0 ? -v : v;
                double c = z - u;

                double p1 = b / 2;
                double d = p1 * p1 - c;

                if (d.IsAlmostEquals(0, Constants.ALGEBRAIC_QUARTIC_SOLVER_QUARTIC_PRECISION))
                {
                    double x = -p1 + resubstitute;
                    switch (roots)
                    {
                        case 0: a_x1 = x; break;
                        case 1: a_x2 = x; break;
                        case 2: a_x3 = x; break;
                        case 3: a_x4 = x; break;
                    }
                    roots++;
                }
                else if (d > 0)
                {
                    d = Math.Sqrt(d);

                    double x = d - p1 + resubstitute;
                    switch (roots)
                    {
                        case 0: a_x1 = x; break;
                        case 1: a_x2 = x; break;
                        case 2: a_x3 = x; break;
                        case 3: a_x4 = x; break;
                    }
                    roots++;

                    x = -d - p1 + resubstitute;
                    switch (roots)
                    {
                        case 0: a_x1 = x; break;
                        case 1: a_x2 = x; break;
                        case 2: a_x3 = x; break;
                        case 3: a_x4 = x; break;
                    }
                    roots++;
                }

                c = z + u;
                b = q < 0 ? v : -v;

                p1 = b / 2;
                d = p1 * p1 - c;

                if (d.IsAlmostEquals(0, Constants.ALGEBRAIC_QUARTIC_SOLVER_QUARTIC_PRECISION))
                {
                    double x = -p1 + resubstitute;
                    switch (roots)
                    {
                        case 0: a_x1 = x; break;
                        case 1: a_x2 = x; break;
                        case 2: a_x3 = x; break;
                        case 3: a_x4 = x; break;
                    }
                    roots++;
                }
                else if (d > 0)
                {
                    d = Math.Sqrt(d);

                    double x = d - p1 + resubstitute;
                    switch (roots)
                    {
                        case 0: a_x1 = x; break;
                        case 1: a_x2 = x; break;
                        case 2: a_x3 = x; break;
                        case 3: a_x4 = x; break;
                    }
                    roots++;

                    x = -d - p1 + resubstitute;
                    switch (roots)
                    {
                        case 0: a_x1 = x; break;
                        case 1: a_x2 = x; break;
                        case 2: a_x3 = x; break;
                        case 3: a_x4 = x; break;
                    }
                    roots++;
                }
            }

            return roots;
        }

        private static double CubicRoot(double a_x)
        {
            if (a_x > 0.0)
                return Math.Pow(a_x, 1.0 / 3.0);
            else if (a_x < 0.0)
                return -Math.Pow(-a_x, 1.0 / 3.0);
            else
                return 0.0;
        }

        public int SolveAlgebraicCubic(ref double a_x1, ref double a_x2, ref double a_x3)
        {
            if (Polynomial.Order < 3)
            {
                int roots2 = SolveAlgebraicQuadratic(ref a_x1, ref a_x2);
                a_x3 = Double.NaN;
                return roots2;
            }

            if ((Polynomial[3] != 1) && (Polynomial[3] != 0))
            {
                return new PolynomialSolverAlgebraic(Polynomial.Normalize()).SolveAlgebraicCubic(
                    ref a_x1, ref a_x2, ref a_x3);
            }

            // Substitute x = y - A/3 to eliminate quadric term: x^3 +px + q = 0
            double A2 = Polynomial[2] * Polynomial[2];
            double p = -A2 / 9 + Polynomial[1] / 3;
            double q = Polynomial[2] * A2 / 27 - Polynomial[2] * Polynomial[1] / 6 + Polynomial[0] / 2;

            double resubstitute = -Polynomial[2] / 3;

            // Use Cardano's formula.

            double P3 = p * p * p;
            double d = q * q + P3;

            if (d.IsAlmostEquals(0, Constants.ALGEBRAIC_QUARTIC_SOLVER_CUBIC_DELTA_PRECISION))
            {
                if (q.IsAlmostEquals(0, Constants.ALGEBRAIC_QUARTIC_SOLVER_QUARTIC_PRECISION))
                {
                    // One triple solution.
                    a_x1 = resubstitute;
                    return 1;
                }
                else
                {
                    // One Single and double solution.
                    double u = CubicRoot(-q);

                    a_x1 = 2 * u + resubstitute;
                    a_x2 = -u + resubstitute;
                    return 2;
                }
            }
            else if (d < 0)
            {
                // Three real solutions.
                double phi = Math.Acos(-q / Math.Sqrt(-P3)) / 3;
                double t = 2 * Math.Sqrt(-p);

                a_x1 = t * Math.Cos(phi) + resubstitute;
                a_x2 = -t * Math.Cos(phi + MathExtensions.PI / 3) + resubstitute;
                a_x3 = -t * Math.Cos(phi - MathExtensions.PI / 3) + resubstitute;
                return 3;
            }
            else
            {
                // One real solution.
                d = Math.Sqrt(d);
                a_x1 = CubicRoot(d - q) - CubicRoot(d + q) + resubstitute;
                return 1;
            }
        }

        public int SolveAlgebraicQuadratic(ref double a_x1, ref double a_x2)
        {
            if (Polynomial.Order < 2)
            {
                a_x1 = SolveAlgebraicLinear();
                a_x2 = Double.NaN;
                return 1;
            }

            double p = Polynomial[1] / Polynomial[2] / 2;
            double q = Polynomial[0] / Polynomial[2];

            double d = p * p - q;

            if (d >= 0)
            {
                d = Math.Sqrt(d);

                a_x1 = d - p;
                a_x2 = -d - p;
                return 2;
            }

            return 0;
        }
    }
}
